using IQIGame.Onigao.Framework;
using RecastUnity;
using System;
using UnityEngine;

namespace IQIGame.Onigao.GamePlay
{
    public abstract class TNavigationController : TModuleController
    {
        private const int MaxObstacleRequestNum = 64;

        protected IntPtr _navMeshScene;
        //复用变量
        protected float[] _halfExtents = new float[3] { 2, 4, 2 };
        protected float[] _startPos = new float[3];
        protected float[] _endPos = new float[3];
        protected float[] _realEndPos = new float[3];
        protected float[] _smoothPath = new float[256 * 3];
        protected float[] _sharedCenter = new float[3];
        /// <summary>
        /// 只用于阻挡计算
        /// </summary>
        protected float[] sharedHalfExtents = new float[3];

        public Bounds bounds { get; protected set; }
        public Rect boundsRect { get; protected set; }
        protected abstract bool isActiveRecastObstacle { get; }

        /// <summary>
        /// 当前添加或删除静态阻挡的请求个数（底层不论添加还是移除一次最多只支持64个）
        /// </summary>
        private int _addOrRemoveObstacleReqNum = 0;

        public bool FindNearestPoint(Vector3 startPos, out Vector3 realEndPos)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                realEndPos = Vector3.zero;
                return false;
            }

            RecastUtility.UnityPos2RecastPos(startPos, _startPos);
            int result = RecastDll.RecastFindNearestPoint(_navMeshScene, _halfExtents, _startPos, _realEndPos);
            realEndPos = RecastUtility.RecastPos2UnityPos(_realEndPos);

            return result >= 0;
        }

        public bool TryMove(Vector3 startPos, Vector3 endPos, out Vector3 realEndPos)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                realEndPos = Vector3.zero;
                return false;
            }

            RecastUtility.UnityPos2RecastPos(startPos, dest: _startPos);
            RecastUtility.UnityPos2RecastPos(endPos, _endPos);

            int result = RecastDll.RecastTryMove(_navMeshScene, _halfExtents, _startPos, _endPos, _realEndPos);
            realEndPos = RecastUtility.RecastPos2UnityPos(_realEndPos);

            return result >= 0;
        }

        public int FindPath(Vector3 startPos, Vector3 targetPos, out float[] path)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                path = null;
                return -1;
            }

            RecastUtility.UnityPos2RecastPos(startPos, _startPos);
            RecastUtility.UnityPos2RecastPos(targetPos, _endPos);

            int smoothPathCount = RecastDll.RecastFindFollow(_navMeshScene, _halfExtents, _startPos, _endPos, _smoothPath);
            path = _smoothPath;
            return smoothPathCount;
        }

        #region 阻挡/动态避障

        public uint AddObstacle(Vector3 position, Vector3 euler, ObstacleInfo info, float agentSpeed)
        {
            if (info == null || info.invalid || info.isClient)
            {
                return 0;
            }
            if (info.isStatic)
            {
                if (_addOrRemoveObstacleReqNum >= MaxObstacleRequestNum)
                {
                    ForceUpdateObstacles();
                    _addOrRemoveObstacleReqNum = 0;
                }
                uint obstacleId;
                if (info.isCapsule)
                {
                    obstacleId = AddCapsuleObstacle(position, info);
                }
                else
                {
                    obstacleId = AddRectObstacle(position, euler, info);
                }
                _addOrRemoveObstacleReqNum++;
                return obstacleId;
            }
            else
            {
                return (uint)AddAgent(position, info, agentSpeed);
            }
        }

        private uint AddRectObstacle(Vector3 position, Vector3 euler, ObstacleInfo info)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return 0;
            }
            uint obstacleId = 0;
            GameMathUtil.ObstacleInfo2RecastBox(position, info, _sharedCenter, sharedHalfExtents, euler.y);
            int result = RecastDll.RecastAddBoxCenterObstacle(_navMeshScene, ref obstacleId, _sharedCenter, sharedHalfExtents, euler.y, out uint errCode);
            if (result < 0)
            {
                LogGame.LogError($"添加矩形阻挡失败。result: {result}, errCode: {errCode}, position: {position}, ObstacleInfo: {info}, yRotation: {euler.y}。");
                return 0;
            }
            return obstacleId;
        }

        private uint AddCapsuleObstacle(Vector3 position, ObstacleInfo info)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return 0;
            }
            uint obstacleId = 0;
            RecastUtility.UnityPos2RecastPos(position, _startPos);
            float obRadius = GameMathUtil.LogicValue2Float(info.radius);
            float obHeight = GameMathUtil.LogicValue2Float(info.height);
            int result = RecastDll.RecastAddObstacle(_navMeshScene, ref obstacleId, _startPos, obRadius, obHeight, out uint errCode);
            if (result < 0)
            {
                LogGame.LogError($"添加胶囊阻挡失败。result: {result}, errCode: {errCode}, position: {position}, ObstacleInfo: {info}。");
                return 0;
            }
            return obstacleId;
        }

        public int AddAgent(Vector3 position, ObstacleInfo info, float speed)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return 0;
            }

            RecastUtility.UnityPos2RecastPos(position, _startPos);
            float obRadius = GameMathUtil.LogicValue2Float(info.radius);
            float obHeight = GameMathUtil.LogicValue2Float(info.height);
            int agentId = RecastDll.RecastAddAgent(_navMeshScene, _startPos, obRadius, obHeight, speed, MainCityConst.AgentMaxAcceleration);
            if (agentId < 0)
            {
                LogGame.LogError($"添加避障代理失败。agentId: {agentId}, position: {position}, ObstacleInfo: {info}, speed: {speed}。");
                return 0;
            }
            //RecastDll的问题，agentId==0有效，而且是底层返回的，但到entity层，不管是agentId还是ObstacleId都算ObstacleId，为了保证合法性，这里要+1转换
            return agentId + 1;
        }

        public void RemoveObstacle(bool isStatic, uint obstacleId)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return;
            }

            if (obstacleId == 0)
            {
                return;
            }
            if (isStatic)
            {
                int result = RecastDll.RecastRemoveObstacle(_navMeshScene, obstacleId, out uint errCode);
                if (result < 0)
                {
                    LogGame.LogError($"移除阻挡失败。result: {result}, errCode: {errCode}, obstacleId: {obstacleId}。");
                }
            }
            else
            {
                int agentId = ObstacleIdToAgentId(obstacleId);
                RecastDll.RecastRemoveAgent(_navMeshScene, agentId);
            }
        }

        /// <summary>
        /// 让agent寻路到目标点（避障模式，根据添加Agent时设置的速度需要一定的时间）
        /// 注意！一旦设置了这个接口，底层一定会将Agent寻路到目的地，即便使用SetAgentPos，寻路也不会停止，除非再次调用AgentHeadTo改变目的地
        /// </summary>
        /// <param name="obstacleId"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        public bool AgentHeadTo(uint obstacleId, Vector3 targetPos)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return false;
            }
            int agentId = ObstacleIdToAgentId(obstacleId);
            RecastUtility.UnityPos2RecastPos(targetPos, _startPos);
            int result = RecastDll.RecastSetAgentMoveTarget(_navMeshScene, agentId, _startPos);
            if (result < 0)
            {
                LogGame.LogError("Recast AgentHeadTo failed, result:" + result);
                return false;
            }
            return true;
        }

        /// <summary>
        /// 强制设置agent的位置，但需要注意，如果之前调用了AgentHeadTo()，再调用此方法，避障寻路并不会停止
        /// 也就是说，如果想要确切的马上将agent停止在指定位置，请调用TeleportAgentTo，或先调用AgentHeadTo(targetPos)，再调用此方法才行
        /// </summary>
        /// <param name="obstacleId"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        public bool SetAgentPos(uint obstacleId, Vector3 targetPos)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return false;
            }
            //先刷新障碍物
            ForceUpdateObstacles();

            int agentId = ObstacleIdToAgentId(obstacleId);
            RecastUtility.UnityPos2RecastPos(targetPos, _startPos);
            int result = RecastDll.RecastSetAgentPos(_navMeshScene, agentId, _startPos);
            if (result < 0)
            {
                LogGame.LogError("Recast SetAgentPos failed, result:" + result);
                return false;
            }
            return true;
        }

        /// <summary>
        /// 传送agent到指定位置，需要停止agent避障寻路，且目标点和当前点不相同，且需要马上生效时调用
        /// </summary>
        /// <param name="obstacleId"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        public bool TeleportAgentTo(uint obstacleId, Vector3 targetPos)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return false;
            }
            int agentId = ObstacleIdToAgentId(obstacleId);
            RecastUtility.UnityPos2RecastPos(targetPos, _startPos);
            //重设导航目标
            int result = RecastDll.RecastSetAgentMoveTarget(_navMeshScene, agentId, _startPos);
            if (result < 0)
            {
                LogGame.LogError("TeleportAgentTo error. RecastSetAgentMoveTarget failed, result:" + result);
                return false;
            }
            //先刷新障碍物
            ForceUpdateObstacles();
            //直接设置agent位置
            result = RecastDll.RecastSetAgentPos(_navMeshScene, agentId, _startPos);
            if (result < 0)
            {
                LogGame.LogError("TeleportAgentTo error. RecastSetAgentPos failed, result:" + result);
                return false;
            }
            return true;
        }

        public Vector3 GetAgentPos(uint obstacleId)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return Vector3.zero;
            }

            int agentId = ObstacleIdToAgentId(obstacleId);
            RecastDll.RecastGetAgentPos(_navMeshScene, agentId, _startPos);
            return RecastUtility.RecastPos2UnityPos(_startPos);
        }

        public bool ChangeAgentSpeed(uint obstacleId, float speed)
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return false;
            }

            int agentId = ObstacleIdToAgentId(obstacleId);
            int result = RecastDll.RecastSetAgentSpeed(_navMeshScene, agentId, speed, MainCityConst.AgentMaxAcceleration);
            if (result < 0)
            {
                LogGame.LogError("Recast ChangeAgentSpeed failed, result:" + result);
                return false;
            }
            return true;
        }

        protected override void OnUpdate(float deltaTime)
        {
            if (!isActiveRecastObstacle)
            {
                return;
            }
            RecastDll.RecastUpdate(_navMeshScene, deltaTime);
            //每帧单个地刷新缓存里add，remove obstacle的数据，性能更平稳
            RecastDll.RecastUpdateObstacles(_navMeshScene, true);
            //由于每帧单个刷新obstacle，所以这里的请求容量数量--
            if (_addOrRemoveObstacleReqNum > 0)
            {
                _addOrRemoveObstacleReqNum--;
            }
        }

        public void ForceUpdateObstacles()
        {
            if (_navMeshScene == IntPtr.Zero)
            {
                return;
            }

            //一帧直接把缓存里待刷新的add，remove obstacle的数据刷完，性能不如isEveryFrame == true平稳
            //如果缓存里无数据，则几乎无消耗
            RecastDll.RecastUpdateObstacles(_navMeshScene, false);
        }

        private int ObstacleIdToAgentId(uint obstacleId)
        {
            //RecastDll的问题，agentId==0有效，而且是底层返回的，但到entity层，不管是agentId还是ObstacleId都算ObstacleId，
            //为了保证合法性，ObstacleId = agentId + 1
            return (int)obstacleId - 1;
        }
        #endregion 阻挡/动态避障

        protected override void OnDispose()
        {
            if (_navMeshScene != IntPtr.Zero)
            {
                RecastDll.RecastClear(_navMeshScene);
                _navMeshScene = IntPtr.Zero;
            }
        }
    }
}
